#include <stdio.h>
#include <stdlib.h>
#include <memory.h>

#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include "hi_io.h"
#include "hi_time.h"
#include "ssd1306.h"
#include "iot_i2c.h"
#include "iot_watchdog.h"
#include "robot_control.h"
#include "iot_errno.h"

#define OLED_I2C_BAUDRATE 400*1000
#define GPIO13 13
#define GPIO14 14
#define FUNC_SDA 6
#define FUNC_SCL 6
extern unsigned char g_car_status;
// float forward_speed = 0.0;
char forward_speed_char_out[20] = "speed:";
extern int g_last_mqtt_command;//从外部文件获取状态1-5
extern int g_last_mqtt_speed;
extern int global_speed;
char speed_char_out[20] = "speed mode:";
char dict[8][10]={"","forward","backward","left","right","stop","left","right"};
// 获取小车方向
typedef enum CarDirection {
    DIR_FORWARD = 1,
    DIR_BACKWARD = 2,
    DIR_LEFT = 3,
    DIR_RIGHT = 4,
    DIR_STOP = 5,
    DIR_BIG_LEFT = 6,
    DIR_BIG_RIGHT = 7,
}CarDir;
extern CarDir car_dir_index;
extern void car_run(CarDir dir);
// 自定义函数，将整数转换为字符串
int numToStr(int num, char chat[]) {
    int i = 0;
    // 处理数字为 0 的情况
    if (num == 0) {
        chat[i++] = '0';
    } else {
        // 逐位提取数字并转换为字符
        while (num > 0) {
            chat[i++] = (num % 10) + '0';
            num /= 10;
        }
    }
    // 反转字符串
    int start = 0;
    int end = i - 1;
    while (start < end) {
        char temp = chat[start];
        chat[start] = chat[end];
        chat[end] = temp;
        start++;
        end--;
    }
    // 添加字符串结束符
    chat[i] = '\0';
    return i;
}

void getSpeedCharOut(void){
    int i = numToStr((1000 - global_speed)/10,speed_char_out+11);
    speed_char_out[i+11] = '%';
    speed_char_out[i+12] = '\0';
}

// void getForwardSpeedCharOut(void){
//     printf("forward_speed:%f\n",forward_speed);
//     int forward_speed_int_len = 0;
//     if(forward_speed<1 && forward_speed>-1){
//         forward_speed = 0.0;
//     }
//     if(forward_speed>99){
//         forward_speed = 99;
//     }
//     if(forward_speed<-99){
//         forward_speed = -99;
//     }
//     if(forward_speed<0){
//         forward_speed_char_out[6] = '-';
//         forward_speed_int_len = numToStr(-forward_speed, forward_speed_char_out + 7) + 1;
//     } else {   
//         forward_speed_int_len = numToStr(forward_speed, forward_speed_char_out + 6);
//     }
//     forward_speed_char_out[6 + forward_speed_int_len] = 'c';
//     forward_speed_char_out[7 + forward_speed_int_len] = 'm';
//     forward_speed_char_out[8 + forward_speed_int_len] = '/';
//     forward_speed_char_out[9 + forward_speed_int_len] = 's';
//     forward_speed_char_out[10 + forward_speed_int_len] = '\0';
// }
void Ssd1306TestTask(void* arg)
{
    (void) arg;
    hi_io_set_func(GPIO13, FUNC_SDA);
    hi_io_set_func(GPIO14, FUNC_SCL);
    IoTI2cInit(0, OLED_I2C_BAUDRATE);

    IoTWatchDogDisable();

    usleep(20*1000);
    ssd1306_Init();
    ssd1306_Fill(Black);
    ssd1306_SetCursor(0, 0);
    ssd1306_DrawString("Car Runs Fast Group!", Font_7x10, White);

    uint32_t start = HAL_GetTick();
    ssd1306_UpdateScreen();
    uint32_t end = HAL_GetTick();
    printf("ssd1306_UpdateScreen time cost: %d ms.\r\n", end - start);
    osDelay(100);

    while (1)
    {
        // printf("g_car_status is %d\r\n",g_car_status);
        ssd1306_Fill(Black);
        ssd1306_SetCursor(10, 10);
        if (g_car_status == CAR_STOP_STATUS)
        {
            ssd1306_DrawString("Robot Car Stop", Font_7x10, White);
        }
        else if (g_car_status == CAR_TRACE_STATUS)
        {
            ssd1306_DrawString("trace", Font_7x10, White);
            // 获取方向
            ssd1306_SetCursor(10, 30);
            ssd1306_DrawString(dict[car_dir_index], Font_7x10, White);
        }
        else if(g_car_status == CAR_OBSTACLE_AVOIDANCE_STATUS) {
            ssd1306_DrawString("ultrasonic", Font_7x10, White);
        }
        else if(g_car_status==CAR_MQTT_CONTROL_STATUS){
            ssd1306_DrawString("remote_control", Font_7x10, White);
            ssd1306_SetCursor(10, 30);
            ssd1306_DrawString(dict[g_last_mqtt_command], Font_7x10, White);
        }
        getSpeedCharOut();//获取速度
        ssd1306_SetCursor(10, 20);
        ssd1306_DrawString(speed_char_out, Font_7x10, White);
        // getForwardSpeedCharOut();
        // ssd1306_SetCursor(10, 40);
        // ssd1306_DrawString(forward_speed_char_out, Font_7x10, White);
        ssd1306_UpdateScreen();
        osDelay(10);
    }
    
}

void Ssd1306TestDemo(void)
{
    osThreadAttr_t attr;

    attr.name = "Ssd1306Task";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = 10240;
    attr.priority = osPriorityNormal;

    if (osThreadNew(Ssd1306TestTask, NULL, &attr) == NULL) {
        printf("[Ssd1306TestDemo] Falied to create Ssd1306TestTask!\n");
    }
}
APP_FEATURE_INIT(Ssd1306TestDemo);
